#include "pid_module.h"
#include "io_module.h"
#include "temp_module.h"
#include "log_module.h"
#include "alarm_module.h"

static pid_config_t *pid_config = RT_NULL;
static rt_thread_t pid_thread = RT_NULL;

/* PID controller internal state */
typedef struct {
    float integral;
    float prev_error;
    float prev_measurement;
    uint32_t last_time;
} pid_state_t;

static pid_state_t pid_states[PID_CONTROLLERS];

static float pid_compute(uint8_t controller_id, float measurement)
{
    pid_controller_t *pid = &pid_config->controllers[controller_id];
    pid_state_t *state = &pid_states[controller_id];
    
    if (!pid->enabled || !pid->auto_mode) {
        return pid->output;
    }
    
    uint32_t current_time = rt_tick_get();
    float dt = (current_time - state->last_time) / 1000.0f;  /* Convert to seconds */
    
    if (dt <= 0.0f) {
        return pid->output;
    }
    
    /* Calculate error */
    float error = pid->setpoint - measurement;
    
    /* Proportional term */
    float proportional = pid->kp * error;
    
    /* Integral term */
    state->integral += error * dt;
    
    /* Integral windup protection */
    if (state->integral > pid->output_max) {
        state->integral = pid->output_max;
    } else if (state->integral < pid->output_min) {
        state->integral = pid->output_min;
    }
    
    float integral_term = pid->ki * state->integral;
    
    /* Derivative term (using measurement to avoid derivative kick) */
    float derivative = (measurement - state->prev_measurement) / dt;
    float derivative_term = pid->kd * derivative;
    
    /* Calculate output */
    pid->output = proportional + integral_term - derivative_term;
    
    /* Output clamping */
    if (pid->output > pid->output_max) {
        pid->output = pid->output_max;
    } else if (pid->output < pid->output_min) {
        pid->output = pid->output_min;
    }
    
    /* Update state */
    state->prev_error = error;
    state->prev_measurement = measurement;
    state->last_time = current_time;
    
    return pid->output;
}

static void pid_control_outputs(void)
{
    float heater_output, compressor_output;
    
    /* Get temperature measurement (using channel 0 as process variable) */
    float pv = temp_get_temperature(0);
    pid_config->controllers[PID_HEATER].input = pv;
    pid_config->controllers[PID_COMPRESSOR].input = pv;
    
    /* Compute PID outputs */
    heater_output = pid_compute(PID_HEATER, pv);
    compressor_output = pid_compute(PID_COMPRESSOR, pv);
    
    /* Control outputs based on PID results */
    if (pid_config->controllers[PID_HEATER].auto_mode) {
        /* Heater control: simple on/off based on output threshold */
        if (heater_output > 50.0f) {  /* 50% output threshold */
            io_set_output_state(0, RT_TRUE);  /* Heater ON */
        } else {
            io_set_output_state(0, RT_FALSE); /* Heater OFF */
        }
    }
    
    if (pid_config->controllers[PID_COMPRESSOR].auto_mode) {
        /* Compressor control: simple on/off based on output threshold */
        if (compressor_output > 50.0f) {  /* 50% output threshold */
            io_set_output_state(1, RT_TRUE);  /* Compressor ON */
        } else {
            io_set_output_state(1, RT_FALSE); /* Compressor OFF */
        }
    }
}

static void process_pid_message(message_t *msg)
{
    if (msg == RT_NULL) return;
    
    switch (msg->msg_type) {
        case MSG_CONTROL_CMD:
            if (msg->data != RT_NULL) {
                if (msg->data_len == sizeof(pid_parameters_t)) {
                    pid_parameters_t *params = (pid_parameters_t *)msg->data;
                    pid_set_parameters(params->controller_id, params->kp, params->ki, params->kd,
                                      params->output_min, params->output_max);
                    pid_set_setpoint(params->controller_id, params->setpoint);
                }
            }
            break;
            
        case MSG_DATA_UPDATE:
            /* Temperature data updated, trigger PID calculation */
            break;
            
        default:
            break;
    }
}

rt_err_t pid_module_init(void)
{
    /* Get configuration pointer */
    pid_config = &g_system_config.pid;
    
    /* Initialize PID states */
    for (int i = 0; i < PID_CONTROLLERS; i++) {
        pid_states[i].integral = 0.0f;
        pid_states[i].prev_error = 0.0f;
        pid_states[i].prev_measurement = 0.0f;
        pid_states[i].last_time = rt_tick_get();
    }
    
    /* Create PID thread */
    pid_thread = rt_thread_create("pid", 
                                 pid_thread_entry, 
                                 RT_NULL, 
                                 2048, 
                                 16, 
                                 20);
    if (pid_thread == RT_NULL) {
        LOG_E("Failed to create PID thread\n");
        return RT_ERR_MEM;
    }
    
    LOG_I("PID module initialized successfully\n");
    return RT_OK;
}

void pid_thread_entry(void *parameter)
{
    rt_uint32_t tick;
    
    while (1) {
        tick = rt_tick_get();
        
        if (pid_config->base.enabled) {
            /* Execute PID control */
            pid_control_outputs();
            
            /* Send output update message */
            send_message(MODULE_MODBUS, MSG_DATA_UPDATE, RT_NULL, 0);
        }
        
        /* Wait for next control cycle */
        rt_thread_delay_until(&tick, rt_tick_from_millisecond(pid_config->controllers[0].control_interval));
    }
}

rt_err_t pid_set_parameters(uint8_t controller_id, float kp, float ki, float kd, 
                           float output_min, float output_max)
{
    if (controller_id >= PID_CONTROLLERS || pid_config == RT_NULL) {
        return RT_ERR_PARAM;
    }
    
    pid_config->controllers[controller_id].kp = kp;
    pid_config->controllers[controller_id].ki = ki;
    pid_config->controllers[controller_id].kd = kd;
    pid_config->controllers[controller_id].output_min = output_min;
    pid_config->controllers[controller_id].output_max = output_max;
    
    /* Reset integral term when parameters change */
    pid_states[controller_id].integral = 0.0f;
    
    LOG_I("PID controller %d parameters updated: Kp=%.3f, Ki=%.3f, Kd=%.3f\n", 
          controller_id, kp, ki, kd);
    
    return RT_OK;
}

rt_err_t pid_set_setpoint(uint8_t controller_id, float setpoint)
{
    if (controller_id >= PID_CONTROLLERS || pid_config == RT_NULL) {
        return RT_ERR_PARAM;
    }
    
    pid_config->controllers[controller_id].setpoint = setpoint;
    
    LOG_I("PID controller %d setpoint changed to: %.1f\n", controller_id, setpoint);
    
    return RT_OK;
}

rt_err_t pid_set_mode(uint8_t controller_id, pid_mode_t mode)
{
    if (controller_id >= PID_CONTROLLERS || pid_config == RT_NULL) {
        return RT_ERR_PARAM;
    }
    
    pid_config->controllers[controller_id].auto_mode = (mode == PID_MODE_AUTO);
    
    LOG_I("PID controller %d mode changed to: %s\n", 
          controller_id, (mode == PID_MODE_AUTO) ? "AUTO" : "MANUAL");
    
    return RT_OK;
}

rt_err_t pid_set_auto_control(rt_bool_t enable)
{
    if (pid_config == RT_NULL) {
        return RT_ERR_PARAM;
    }
    
    pid_config->controllers[PID_HEATER].auto_mode = enable;
    pid_config->controllers[PID_COMPRESSOR].auto_mode = enable;
    
    LOG_I("PID auto control %s\n", enable ? "enabled" : "disabled");
    
    return RT_OK;
}

float pid_get_output(uint8_t controller_id)
{
    if (controller_id >= PID_CONTROLLERS || pid_config == RT_NULL) {
        return 0.0f;
    }
    
    return pid_config->controllers[controller_id].output;
}

float pid_get_setpoint(uint8_t controller_id)
{
    if (controller_id >= PID_CONTROLLERS || pid_config == RT_NULL) {
        return 0.0f;
    }
    
    return pid_config->controllers[controller_id].setpoint;
}

rt_bool_t pid_get_auto_control(void)
{
    if (pid_config == RT_NULL) {
        return RT_FALSE;
    }
    
    return pid_config->controllers[PID_HEATER].auto_mode;
}

pid_config_t *pid_get_config(void)
{
    return pid_config;
}